/**********************************************
*  Namespace: AI
*  Classes:   SteeringArrive
*  Desc:      Updates positions using a collision
*             avoidance algorithm
*  Author:    Vaios Kalpias-Ilias (C) 2008-2009
***********************************************/

#include "SteeringAvoidance.h"
#include "SteeringEvade.h"
#include "SteeringSeek.h"
#include "SteeringPursue.h"
#include "Helper.h"

namespace AI
{
	//---------------------------------------------
	/**
	* Ctor
	*/
	SteeringAvoidance::SteeringAvoidance()
	{
	}
	//---------------------------------------------
	/**
	* Dtor
	*/
	SteeringAvoidance::~SteeringAvoidance()
	{
	}
	//---------------------------------------------
	/**
	* Returns new kinematic data in 2d
	* @param current
	* @param target
	*/
	Steering2d SteeringAvoidance::GetSteering(const Kinematic2d& current,
			 const Kinematic2d targets[], int numOfTargets )
	{		
		// store the first collision time
		float shortestTime = mth::INFINITY;
	
		// initialize helper variables
		const Kinematic2d* firstTarget = 0;
		float firstMinSeparation = 0.0f;
		float firstDistance = 0.0f;
		mth::Vector2 firstRelativePos;
		mth::Vector2 firstRelativeVel;

		// loop through targets
		for (int i=0; i<numOfTargets; ++i)
		{
			// do not check with self
			if (&current == &targets[i])
				continue;

			// calculate the time to collision
			mth::Vector2 relativePos(targets[i].position - current.position);
			mth::Vector2 relativeVel(targets[i].velocity - current.velocity);
			float relativeSpeed = relativeVel.Length();
			float timeToCollision = 0.0f;
			if (relativeSpeed > 0.0f)
			{
				timeToCollision = (relativePos * relativeVel)/
					 (relativeSpeed * relativeSpeed);
			}
			

			// check if it is going to be a collision at all
			float distance = relativePos.Length();
			float minSeparation = distance - relativeSpeed * shortestTime;
			if (minSeparation > 2.0f * collisionRadius)
				continue;

			// check if it is the shortest
			if (timeToCollision > 0.0f && timeToCollision < shortestTime)
			{
				// store the time, target and other data
				shortestTime = timeToCollision;
				firstTarget = &targets[i];
				firstMinSeparation = minSeparation;
				firstDistance = distance;
				firstRelativePos = relativePos;
				firstRelativeVel = relativeVel;
			}
		}

		// if we have no target then exit
		if (!firstTarget)
			return Steering2d();

		// if we are going to hit exactly, or if we're already colliding
		// then do the steering based on current position
		mth::Vector2 relativePos;
		if (firstMinSeparation <= 0.0f || firstDistance < 2.0f*collisionRadius)
		{
			relativePos = firstTarget->position - current.position;
		}
		// otherwise calculate the future relative pos
		else
		{
			relativePos = firstRelativePos + firstRelativeVel * shortestTime;
		}

		// avoid the target
		relativePos.Normalize();
		Steering2d steering(relativePos * maxAcceleration, 0.0f);
		return steering;
	}

}
